/*
 * usb_cmd_handler.c
 * Sample Command 1:
 * "A,30,-30,30,-30\r"
 * Means moving Stepper 1,2,3,4 of Pan-Tilt-Roll(PTR) A to 45, -45, 30, -30 deg accordingly
 *
 * Pan-Tilt-Roll : From A to F, for Antenna A to F
 * Degree : -45 to +45, no decimals allowed
 *
 *  Created on: Mar 22, 2025
 *      Author: wzx05
 */

#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include "usb_cmd_handler.h"
#include "usbd_cdc_if.h" // 如果需要调用 CDC_Transmit_FS 等函数
#include <Motor\stepper_cfg.h> // 需要调用给全局电机组相关属性
#include <Motor\stepper.h>

#define DEG_MIN -45
#define DEG_MAX 45

#define MAX_COMMAND_SIZE 32
static uint8_t commandBuffer[MAX_COMMAND_SIZE] = {0}; // 初始化时全填充 0
static uint32_t commandLength = 0;

// 将接收的UTF-8字节转换为ASCII字符（假设输入为可打印ASCII）
static inline char byteToChar(uint8_t byte) {
    return (char)byte;
}

static CmdResult parseStepperCmd(uint8_t* cmdBuffer, uint32_t length, StepperCmd *params) {
    //CmdResult result = CMD_SUCCESS;
    const char *ptr = (const char*)cmdBuffer;
    uint8_t paramCount = 0;
    float angle;

    // Step 1: Find command termination '\r'
    //const char *endPtr = strchr(ptr, '\0');
    if (length < 2) {
        return CMD_ERR_SHORT_COMMAND;
    }

    // Step 2: Extract group (A-F)
    if (!isalpha(*ptr)) {
        return CMD_ERR_GROUP;
    }
    params->group = toupper(*ptr) - 'A';
    ptr++;

    if ((params->group <0) || (params->group > (MOTOR_GROUPS - 1))) { // A=0, D=3
        return CMD_ERR_GROUP;
    }

    // Step 3: Extract parameters
    while (paramCount < MOTORS_PER_GROUP && (*ptr != '\0')) {
        // Skip commas and whitespace
        while (*ptr == ',' || isspace(*ptr)) {
            ptr++;
        }

        // Parse float value
        char *endPtr;
        angle = strtod(ptr, &endPtr);

        // Check conversion validity
        if (endPtr == ptr){
        	return CMD_ERR_DEG_VALUE;
        }

        if ((*endPtr != ',') && !isspace(*endPtr) && (*endPtr != '\0')) {
            return CMD_ERR_FORMAT;
        }

        // Validate range
        if (angle < DEG_MIN || angle > DEG_MAX) {
            return CMD_ERR_DEG_VALUE;
        }

        params->degrees[paramCount++] = angle;

        ptr = endPtr; // Move to next parameter
    }

    // Final checks
    if (paramCount != MOTORS_PER_GROUP) {
    	return CMD_ERR_PARAM_COUNT;
    }

    if (*(ptr) != '\0') {
        return CMD_ERR_FORMAT;
    }

    return CMD_SUCCESS;
}

static void processCommand(uint8_t* buffer, uint32_t length) {
    StepperCmd cmdParams;
    static char response[512];
	uint8_t m;

    while (length > 0 && isspace(buffer[length-1])) {
        length--;
    }

    CmdResult result = parseStepperCmd(buffer, length, &cmdParams);
    //CDC_Transmit_FS(buffer, length);

    //snprintf(response, sizeof(response),"\r\n");
    //CDC_Transmit_FS((uint8_t*)response, strlen(response));
    //CDC_Transmit_FS((uint8_t*)response, strlen(response));

    switch(result) {
        case CMD_SUCCESS:
        	for (m = 0; m < MOTORS_PER_GROUP; m++) {
        		Stepper_MoveToAngle(cmdParams.group, m, cmdParams.degrees[m]);
        	}

            // 预计算组字母（直接使用 .）
            const char group_char = 'A' + cmdParams.group;

            // 显式转换浮点数到整数（避免直接指针强制转换）
            int degrees_int[MOTORS_PER_GROUP];
            for (int m = 0; m < MOTORS_PER_GROUP; m++) {
                degrees_int[m] = (int)cmdParams.degrees[m]; // 截断或四舍五入
            }

            // 批量构建响应字符串
            size_t batch_pos = 0;
            for (int m = 0; m < MOTORS_PER_GROUP; m++) {
                batch_pos += snprintf(
                    response + batch_pos,
                    sizeof(response) - batch_pos,
                    "Move Stepper %d of Group %c to %d\r\n",
                    m,
                    group_char,
                    degrees_int[m]
                );
                if (batch_pos >= sizeof(response)) break; // 溢出保护
            }

            // 一次性发送全部数据（用 batch_pos 而非 strlen）
            CDC_Transmit_FS((uint8_t*)response, batch_pos);
            break;

        case CMD_ERR_GROUP:
            snprintf(response, sizeof(response),"Error: Invalid Motor Group %c\r\n", byteToChar(buffer[0]));
        	//snprintf(response, sizeof(response),"Error: Invalid Motor Group.\r\n");
        	CDC_Transmit_FS((uint8_t*)response, strlen(response));
            break;

        case CMD_ERR_DEG_VALUE:
        	snprintf(response, sizeof(response), "Error: Angle out of range (%d to %d)\r\n", DEG_MIN, DEG_MAX);
            CDC_Transmit_FS((uint8_t*)response, strlen(response));
        	break;

        case CMD_ERR_PARAM_COUNT:
        	snprintf(response, sizeof(response), "Error: Incorrect parameter count\r\n");
            CDC_Transmit_FS((uint8_t*)response, strlen(response));
        	//snprintf(response, sizeof(response), "Sample Command： A,-10,10,-20,20\r\n");
            //CDC_Transmit_FS((uint8_t*)response, strlen(response));
            break;

        case CMD_ERR_SHORT_COMMAND:
        	snprintf(response, sizeof(response), "Error: Command too short.\r\n");
            CDC_Transmit_FS((uint8_t*)response, strlen(response));
        	//snprintf(response, sizeof(response), "Sample Command： A,-10,10,-20,20\r\n");
            //CDC_Transmit_FS((uint8_t*)response, strlen(response));
            break;

        default:
        	snprintf(response, sizeof(response), "Error: Invalid command format\r\n");
            CDC_Transmit_FS((uint8_t*)response, strlen(response));
        	//snprintf(response, sizeof(response), "Sample Command： A,-10,10,-20,20\r\n");
            //CDC_Transmit_FS((uint8_t*)response, strlen(response));
    }
}

void handleCommand(uint8_t* data, uint32_t length) {
    for (uint32_t i = 0; i < length; i++) {
        if (data[i] == '\r' || commandLength >= MAX_COMMAND_SIZE) {
            //commandBuffer[commandLength++] = '\r'; // 将数据添加到命令缓冲区
            processCommand(commandBuffer, commandLength);
            commandLength = 0; // 重置命令缓冲区
            memset(commandBuffer, 0, sizeof(commandBuffer)); // 全填充 0
        } else {
            commandBuffer[commandLength++] = data[i]; // 将数据添加到命令缓冲区
        }
    }
}
